#include "CSphere.h"


boolean CSphere::Sweep( const CVector3d& pos1, fixed r1, const CVector3d& pos2, fixed r2,
					    const CVector3d& v1, const CVector3d& v2, fixed &t, CVector3d& pt )
{
	CVector3d relPos = pos2 - pos1;
	CVector3d relVel = v2 - v1;
	fixed tD = r1 + r2;

	fixed c = relPos * relPos - CMathFixed::Mul( tD, tD );

	if ( c < 0 )
	{
		t = 0;
		return TRUE;
	}

	fixed a = relVel * relVel;
	if ( a < CMathFixed_FloatToFixed( 0.001f ) /* EPSILON */ ) // spheres not moving relative to each other
		return FALSE;

	fixed b = relVel * relPos;

	if ( b >= 0 )  // spheres not moving towards each other
		return FALSE;

	fixed d = CMathFixed::Mul( b, b ) - CMathFixed::Mul( a, c );
	// the discrimant is not b^2 - 4ac because we are solving the exaction ax^2 + 2bx + c = 0 and not ax^2 + bx + c = 0

	if ( d < 0 ) // no real-valued root, no intersection
		return FALSE;

	t = ( -b - CMathFixed::Div( CMathFixed::Sqrt( d ), a ) );
	
	relPos.Normalize(); // normalise relPos

	pt = pos2 + t * relVel + r2 * relPos; // point of contact
	return TRUE;
}


boolean CSphere::Sweep( const CVector3d& pos, fixed r, const CPlane& plane,
					    const CVector3d& v, fixed &t, CVector3d& pt )
{
	fixed dist = plane.GetSignedDistanceOfPoint( pos );
	if ( CMathFixed::Abs( dist ) <= r ) // the sphere already overlaps the plane
	{
		t = 0;
		pt = pos;
		return TRUE;
	}

	fixed denom = plane.m_n * v;
	if ( CMathFixed::Mul( denom, dist ) >= 0 )
	{
		// no intersection as the sphere is moving paralallel or away from the plane
		return FALSE;
	}

	r = ( dist > 0 ) ? r : -r;
	t = CMathFixed::Div( r - dist, denom );
	pt = pos + t * v - r * plane.m_n;
	return TRUE;
}


boolean	CSphere::Intersects( const CVector3d& pos1, fixed r1, const CVector3d& pos2, fixed r2 )
{
	CVector3d d = pos2 - pos1;
	fixed tD = r1 + r2;
	tD = CMathFixed::Mul( tD, tD );

	if ( d * d <= tD )
		return TRUE;

	return FALSE;
}


boolean	CSphere::Intersects( const CVector3d& pos, fixed r, const CVector3d& segA, const CVector3d& segB )
{
	CVector3d closestPt;
	CLineSegment3d::ClosestPoint( segA, segB, pos, closestPt );

	closestPt -= pos;

	if ( closestPt * closestPt <= CMathFixed::Mul( r, r ) )
		return TRUE;

	return FALSE;
	
}


boolean	CSphere::Intersects( const CVector3d& pos, fixed r, const CPlane& p, CSpace::Relativity& relativity )
{
	fixed dist = p.GetSignedDistanceOfPoint( pos );

	if( dist < -r )
	{
		relativity = CSpace::Outside;
		return FALSE;
	}
	
	if( dist > r )
	{
		relativity = CSpace::Inside;
		return FALSE;
	}
	
	
	relativity = CSpace::Intersecting;
	return TRUE;
}
